#include "PhysicsFactory.h"
// in this class you can't enable the memory leak tracer, so keep all bullets
//construction commands here, and keep track of memory leaks

PhysicsFactory * PhysicsFactory::m_instance = NULL;

PhysicsFactory::PhysicsFactory()
{
	m_collisionShapes = new vector<btCollisionShape*>;
	m_rigidBodies = new vector<btRigidBody*>;
	m_constraints = new vector<btTypedConstraint *>;
	m_world = NULL;
}

PhysicsFactory::~PhysicsFactory()
{
	int length;

	length = m_rigidBodies->size();
	for(int i = 0; i < length; i++)
	{
		btRigidBody *t_body = (*m_rigidBodies)[i];		
		
		int refs = t_body->getNumConstraintRefs();
		for(int i=0;i < refs; i++)
		{
			t_body->removeConstraintRef(t_body->getConstraintRef(0));
		}
		
		delete t_body;
		t_body = NULL;
			
	}

	delete m_rigidBodies;
	m_rigidBodies = NULL;

	length = m_constraints->size();
	for(int i = 0; i < length; i++)
	{
		btTypedConstraint *t_constraint = (*m_constraints)[i];		
		delete t_constraint;
		t_constraint = NULL;
			
	}

	delete m_constraints;
	m_constraints = NULL;

	

	
}

PhysicsFactory * PhysicsFactory::GetInstance()
{
	if(m_instance == NULL)
	{
		m_instance = new PhysicsFactory();
	}

	return m_instance;
}

btRigidBody * PhysicsFactory::CreateSquareRigidBody(float mass, float posX, float posY, float posZ, float rotX,float rotY, float rotZ, float rotW,float halfWidth, float halfHeight, float halfDepth)
{
	bool isDynamic = (mass != 0.f);
	btCollisionShape *shape = CreateSquareShape(halfWidth, halfHeight, halfDepth);

	btVector3 localInertia(0,0,0);
	if (isDynamic)
		shape->calculateLocalInertia(mass,localInertia);

	btTransform transform;
	transform.setIdentity();
	transform.setOrigin(btVector3(btScalar(posX), btScalar(posY), btScalar(posZ)));
	transform.setRotation(btQuaternion(btScalar(rotX), btScalar(rotY),btScalar(rotZ),btScalar(rotW)));	

	btDefaultMotionState* myMotionState = new btDefaultMotionState(transform);
	
	btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,shape,localInertia);
	btRigidBody* body = new btRigidBody(rbInfo);

	m_rigidBodies->push_back(body);

	return body;
}

btRigidBody * PhysicsFactory::CreateRigidBody(float mass, float posX, float posY, float posZ, float rotX, float rotY, float rotZ, float rotW, btCollisionShape *a_shape)
{
	bool isDynamic = (mass != 0.f);
	
	btVector3 localInertia(0,0,0);
	if (isDynamic)
		a_shape->calculateLocalInertia(mass,localInertia);

	btTransform transform;
	transform.setIdentity();
	transform.setOrigin(btVector3(btScalar(posX), btScalar(posY), btScalar(posZ)));
	transform.setRotation(btQuaternion(btScalar(rotX), btScalar(rotY),btScalar(rotZ),btScalar(rotW)));	

	btDefaultMotionState* myMotionState = new btDefaultMotionState(transform);
	
	btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,a_shape,localInertia);
	btRigidBody* body = new btRigidBody(rbInfo);

	m_rigidBodies->push_back(body);

	return body;
}

btRigidBody * PhysicsFactory::CreateRigidBody(float mass, btTransform *trans, btCollisionShape *a_shape)
{
	bool isDynamic = (mass != 0.f);

	btVector3 localInertia(0,0,0);
	if (isDynamic)
		a_shape->calculateLocalInertia(mass,localInertia);

	btDefaultMotionState* myMotionState = new btDefaultMotionState(*trans);

	btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,a_shape,localInertia);
	btRigidBody* body = new btRigidBody(rbInfo);

	m_rigidBodies->push_back(body);

	return body;
}

btCollisionShape * PhysicsFactory::CreateSquareShape(float halfWidth, float halfHeight, float halfDepth)
{
	btBoxShape *t_shape = new btBoxShape(btVector3(btScalar(halfWidth),btScalar(halfHeight),btScalar( halfDepth)));	
	m_collisionShapes->push_back(t_shape);
	return t_shape;
}

btCollisionShape * PhysicsFactory::CreateSquareShape(btVector3 *vec)
{
	btBoxShape *t_shape = new btBoxShape(*vec);	
	m_collisionShapes->push_back(t_shape);
	return t_shape;
}

btCollisionShape * PhysicsFactory::CreateCapsuleShape(float radius, float height)
{
	btCapsuleShape *t_shape = new btCapsuleShape(btScalar(radius),btScalar(height));	
	m_collisionShapes->push_back(t_shape);
	return t_shape;
}

btCollisionShape * PhysicsFactory::CreatePlaneShape(float x, float y, float z, float w)
{
	btStaticPlaneShape * t_shape = new btStaticPlaneShape(btVector3(x,y,z),w);	
	m_collisionShapes->push_back(t_shape);
	return t_shape;
}

btDynamicsWorld * PhysicsFactory::CreateWorld(float aabbMinX, float aabbMinY, float  aabbMinZ, float  aabbMaxX, float  aabbMaxY, float aabbMaxZ, float gravityX, float gravityY, float gravityZ)
{
	m_collisionConfiguration = new btDefaultCollisionConfiguration();
	m_constraintSolver = new btSequentialImpulseConstraintSolver();

	btVector3 worldAabbMin(aabbMinX, aabbMinY, aabbMinZ);
	btVector3 worldAabbMax(aabbMaxX, aabbMaxY, aabbMaxZ);
	const int maxProxies = 32766;
	
	m_broadPhaseInterface = new btAxisSweep3(worldAabbMin, worldAabbMax, maxProxies);

	m_collisionDispatcher = new btCollisionDispatcher(m_collisionConfiguration);

	m_world = new btDiscreteDynamicsWorld(m_collisionDispatcher, m_broadPhaseInterface, m_constraintSolver, m_collisionConfiguration);		
	m_world->setGravity(btVector3(gravityX,gravityY,gravityZ));

	return m_world;
}

btDynamicsWorld * PhysicsFactory::GetWorld()
{
	return m_world;
}


btTypedConstraint *  PhysicsFactory::CreateHingeConstraint(btRigidBody *a_body,btRigidBody *b_body, btVector3 *pivotPosInA, btVector3 *pivotPosInB, btVector3 *axisInA, btVector3 *axisInB, float minLimit, float maxLimit)
{
	btHingeConstraint *hingeC = new btHingeConstraint(*a_body, *b_body, *pivotPosInA, *pivotPosInB, *axisInA, *axisInB);			
	hingeC->setLimit(minLimit, maxLimit);
	m_constraints->push_back(hingeC);

	return hingeC;
}

btTypedConstraint *  PhysicsFactory::CreateHingeConstraint(btRigidBody *a_body,btRigidBody *b_body, btTransform *localA, btTransform *localB, float limit)
{		
	
	btHingeConstraint *hingeC = new btHingeConstraint(*a_body, *b_body, *localA, *localB);			
	
	hingeC->setLimit(-limit, limit);
	
	m_constraints->push_back(hingeC);

	return hingeC;
	
}

btTypedConstraint *  PhysicsFactory::CreateHingeConstraint(btRigidBody *a_body,btRigidBody *b_body, btTransform *localA, btTransform *localB, float minLimit, float maxLimit)
{		
	
	btHingeConstraint *hingeC = new btHingeConstraint(*a_body, *b_body, *localA, *localB);			
		
	
	hingeC->setLimit(minLimit, maxLimit);
	
	m_constraints->push_back(hingeC);

	return hingeC;
	
}


btTypedConstraint *  PhysicsFactory::CreateTwistConstraint(btRigidBody *a_body,btRigidBody *b_body, btTransform *localA, btTransform *localB, float twistSpan, float swingSpan1, float swingSpan2)
{
	
	btConeTwistConstraint *twistC = new btConeTwistConstraint(*a_body, *b_body, *localA, *localB);
	
	
	twistC->setLimit(swingSpan1, swingSpan2, twistSpan);
	
	m_constraints->push_back(twistC);
	
	return twistC;
	
}



btTypedConstraint *  PhysicsFactory::CreatePoint2PointConstraint(btRigidBody *a_body,btRigidBody *b_body, btTransform *localA, btTransform *localB)
{		

	btPoint2PointConstraint *pointC = new btPoint2PointConstraint(*a_body, *b_body, localA->getOrigin(), localB->getOrigin());
	m_constraints->push_back(pointC);

	return pointC;
}

btTypedConstraint *  PhysicsFactory::CreatePoint2PointConstraint(btRigidBody *a_body,btRigidBody *b_body, btVector3 *pivotPosInA, btVector3 *pivotPosInB)
{		

	btPoint2PointConstraint *pointC = new btPoint2PointConstraint(*a_body, *b_body, *pivotPosInA, *pivotPosInB);
	
	m_constraints->push_back(pointC);

	return pointC;
	
}

void PhysicsFactory::AddRigidBodyToWorld(btRigidBody* a_body)
{
	m_world->addRigidBody(a_body);
}
void PhysicsFactory::RemoveRigidBodyFromWorld(btRigidBody* a_body)
{
	m_world->removeRigidBody(a_body);
}
void PhysicsFactory::AddConstraintToWorld(btTypedConstraint *a_constraint)
{
	m_world->addConstraint(a_constraint);
}
void PhysicsFactory::AddConstraintToWorld(btTypedConstraint *a_constraint, bool disableCollisionBetweenConnectedBodies)
{
	m_world->addConstraint(a_constraint, disableCollisionBetweenConnectedBodies);
}
void PhysicsFactory::RemoveConstraintFromWorld(btTypedConstraint *a_constraint)
{
	m_world->removeConstraint(a_constraint);
}

void PhysicsFactory::DestroyRigidBody(btRigidBody *a_body)
{
	m_world->removeRigidBody(a_body);
	int length = m_rigidBodies->size();
	for(int i = 0; i < length; i++)
	{
		btRigidBody *t_body = (*m_rigidBodies)[i];
		if(a_body == t_body)
		{
			//delete t_body;
			//t_body = NULL;
			m_rigidBodies->erase(m_rigidBodies->begin() + i);
			return;
		}
	}
}
void PhysicsFactory::DestroyConstraint(btTypedConstraint *a_constraint)
{
	m_world->removeConstraint(a_constraint);
	int length = m_constraints->size();
	for(int i = 0; i < length; i++)
	{
		btTypedConstraint *t_constraint = (*m_constraints)[i];
		if(a_constraint == t_constraint)
		{
			//delete t_body;
			//t_body = NULL;
			m_constraints->erase(m_constraints->begin() + i);
			return;
		}
	}
}